import numpy as np
import matplotlib.pyplot as plt


plt.rcParams['axes.unicode_minus'] = False  # 解决负号显示问题
plt.rc('font', size=10)
plt.rc('font', family='SimHei')

# 通用常数
interval = 1  # 投弹时间间隔
v_down = 3  # 云团下降速度
r_effective = 10  # 有效遮蔽半径
t_effective = 20  # 有效遮蔽时间
v_missile = 300  # 导弹速度
r_target = 7  # 目标半径
H_target = 10  # 目标高度
target = np.array([0, 200, 0])  # 目标底部圆心坐标
M1 = np.array([20000, 0, 2000])
M2 = np.array([19000, 600, 2100])
M3 = np.array([18000, -600, 1900])
FY1 = np.array([17800, 0, 1800])
FY2 = np.array([12000, 1400, 1400])
FY3 = np.array([6000, -3000, 700])
FY4 = np.array([11000, 2000, 1800])
FY5 = np.array([13000, -2000, 1300])
v_range = np.array([70, 140])  # 无人机速度范围
g = 9.8  # 重力加速度
# target_point = np.array([
#     [(1 / 2) ** 0.5 * r_target, (1 / 2) ** 0.5 * r_target + 200, 0],
#     [-(1 / 2) ** 0.5 * r_target, (1 / 2) ** 0.5 * r_target + 200, 0],
#     [(1 / 2) ** 0.5 * r_target, -(1 / 2) ** 0.5 * r_target + 200, 0],
#     [-(1 / 2) ** 0.5 * r_target, -(1 / 2) ** 0.5 * r_target + 200, 0],
#     [(1 / 2) ** 0.5 * r_target, (1 / 2) ** 0.5 * r_target + 200, H_target],
#     [-(1 / 2) ** 0.5 * r_target, (1 / 2) ** 0.5 * r_target + 200, H_target],
#     [(1 / 2) ** 0.5 * r_target, -(1 / 2) ** 0.5 * r_target + 200, H_target],
#     [-(1 / 2) ** 0.5 * r_target, -(1 / 2) ** 0.5 * r_target + 200, H_target],
#     [(1 / 2) ** 0.5 * r_target, (1 / 2) ** 0.5 * r_target + 200, H_target / 2],
#     [-(1 / 2) ** 0.5 * r_target, (1 / 2) ** 0.5 * r_target + 200, H_target / 2],
#     [(1 / 2) ** 0.5 * r_target, -(1 / 2) ** 0.5 * r_target + 200, H_target / 2],
#     [-(1 / 2) ** 0.5 * r_target, -(1 / 2) ** 0.5 * r_target + 200, H_target / 2],
# ])
target_point = np.array([
    [0, 200, 0]
])
t_total = np.linalg.norm(M1) / 300  # 从0开始到导弹击中目标的总用时
precision = 0.01  # 时间精度

# 传入M1的初始坐标（也即可确定其飞行方向）和经过的时间，返回其在给定时间下的坐标
def get_M1_coordinate(coordinate_0, t, v=None):
    if not v:
        v = -coordinate_0 / np.linalg.norm(coordinate_0) * v_missile
    else:
        v = v / np.linalg.norm(v) * v_missile
    return coordinate_0 + v * t


# 传入FY1的坐标和经过的时间，返回它投射的第i枚烟雾弹中心的坐标
def get_FY1_coordinate(coordinate_0, t, i, v_1, t_1, t_2, theta):
    v = np.array([np.cos(theta), np.sin(theta), 0])

    if t < t_1 + i * t_2 or t > t_1 + i * t_2 + t_effective:
        return [None, None, None]
    else:
        v[2] = 0
        v = v / np.linalg.norm(v) * v_1
        # print('v',v)
        delta_t = t - t_1 - t_2
        coordinate = coordinate_0 + v * (t_1 + t_2)
        coordinate[2] -= 1 / 2 * g * (t_2 ** 2)
        coordinate[2] -= delta_t * v_down
        return coordinate


def collision_detection(coordinate_missile, coordinate_bomb):
    if coordinate_bomb[0] == None:
        return False
    global target_point, r_effective
    count_collision = 0
    for target in target_point:
        l_1 = coordinate_bomb - target
        l_2 = coordinate_missile - target
        molecule = np.cross(l_1, l_2)
        denominator = np.linalg.norm(l_2)
        d = np.linalg.norm(molecule) / denominator
        if d < r_effective:
            missile_to_target = -coordinate_missile  # 由导弹指向目标
            missile_to_bomb = coordinate_bomb - coordinate_missile  # 由导弹指向烟幕弹
            cos = np.dot(missile_to_target, missile_to_bomb)
            if cos > 0 or np.linalg.norm(missile_to_bomb) < r_effective:  # 余弦值大于0，是锐角，或者导弹和烟幕弹之间的距离小于有效半径
                count_collision += 1
    return count_collision


# 飞行方向、飞行速度、烟幕干扰弹投放点、烟幕干扰弹起爆点
def fun(theta, v_1, t_1, t_2, FY, M):
    t_list = np.arange(t_1 + t_2, t_1 + t_2 + t_effective, precision)  # 时间序列，终点是0
    count = 0
    for t in t_list:
        if collision_detection(get_M1_coordinate(M, t), get_FY1_coordinate(FY, t, 1, v_1, t_1, t_2, theta)) == len(target_point):
            count += 1
    return count


theta = [3.1081728266336626, 5.486191336231527, 1.201616587425639, 3.2419605638564053, 1.6839337639981466]
v_1 = [100.11003875871468, 104.40620209128916, 83.50408899138267, 123.63513897094941, 121.98407734849373]
t_1 = [[0.7717977984428419, 1.7748945123565458, 3.0586466321232515], [11.819265007461421, 12.900295521066266, 14.81444661792537], [36.85860006416334, 39.72528932444474, 41.50578635239483], [34.345287666139555, 36.03264762927274, 40.82604550958944], [11.406572811355334, 15.082127362479177, 17.756136188358]]
t_2 = [[3.1237964628766752, 3.179136379211439, 2.488898459585851], [4.734975690492209, 4.753174983866941, 4.6767171681763475], [3.937241127232442, 4.558603739957058, 3.9568478159413414], [18.232866818180483, 3.9478536558896313, 4.834276476038372], [3.746309713524477, 1.049019094765241, 2.5834913622090556]]


print(f"无人机FY1第一枚烟幕弹对导弹M1遮蔽时间为{fun(theta[0], v_1[0], t_1[0][0], t_2[0][0], FY1, M1)}")
print(f"无人机FY2第一枚烟幕弹对导弹M1遮蔽时间为{fun(theta[1], v_1[1], t_1[1][0], t_2[1][0], FY2, M1)}")
print(f"无人机FY3第一枚烟幕弹对导弹M1遮蔽时间为{fun(theta[2], v_1[2], t_1[2][0], t_2[2][0], FY3, M1)}")
print(f"无人机FY4第一枚烟幕弹对导弹M1遮蔽时间为{fun(theta[3], v_1[3], t_1[3][0], t_2[3][0], FY4, M1)}")
print(f"无人机FY5第一枚烟幕弹对导弹M1遮蔽时间为{fun(theta[4], v_1[4], t_1[4][0], t_2[4][0], FY5, M1)}")
print(f"无人机FY1第一枚烟幕弹对导弹M2遮蔽时间为{fun(theta[0], v_1[0], t_1[0][0], t_2[0][0], FY1, M2)}")
print(f"无人机FY2第一枚烟幕弹对导弹M2遮蔽时间为{fun(theta[1], v_1[1], t_1[1][0], t_2[1][0], FY2, M2)}")
print(f"无人机FY3第一枚烟幕弹对导弹M2遮蔽时间为{fun(theta[2], v_1[2], t_1[2][0], t_2[2][0], FY3, M2)}")
print(f"无人机FY4第一枚烟幕弹对导弹M2遮蔽时间为{fun(theta[3], v_1[3], t_1[3][0], t_2[3][0], FY4, M2)}")
print(f"无人机FY5第一枚烟幕弹对导弹M2遮蔽时间为{fun(theta[4], v_1[4], t_1[4][0], t_2[4][0], FY5, M2)}")
print(f"无人机FY1第一枚烟幕弹对导弹M3遮蔽时间为{fun(theta[0], v_1[0], t_1[0][0], t_2[0][0], FY1, M3)}")
print(f"无人机FY2第一枚烟幕弹对导弹M3遮蔽时间为{fun(theta[1], v_1[1], t_1[1][0], t_2[1][0], FY2, M3)}")
print(f"无人机FY3第一枚烟幕弹对导弹M3遮蔽时间为{fun(theta[2], v_1[2], t_1[2][0], t_2[2][0], FY3, M3)}")
print(f"无人机FY4第一枚烟幕弹对导弹M3遮蔽时间为{fun(theta[3], v_1[3], t_1[3][0], t_2[3][0], FY4, M3)}")
print(f"无人机FY5第一枚烟幕弹对导弹M3遮蔽时间为{fun(theta[4], v_1[4], t_1[4][0], t_2[4][0], FY5, M3)}")

print(f"无人机FY1第二枚烟幕弹对导弹M1遮蔽时间为{fun(theta[0], v_1[0], t_1[0][1], t_2[0][1], FY1, M1)}")
print(f"无人机FY2第二枚烟幕弹对导弹M1遮蔽时间为{fun(theta[1], v_1[1], t_1[1][1], t_2[1][1], FY2, M1)}")
print(f"无人机FY3第二枚烟幕弹对导弹M1遮蔽时间为{fun(theta[2], v_1[2], t_1[2][1], t_2[2][1], FY3, M1)}")
print(f"无人机FY4第二枚烟幕弹对导弹M1遮蔽时间为{fun(theta[3], v_1[3], t_1[3][1], t_2[3][1], FY4, M1)}")
print(f"无人机FY5第二枚烟幕弹对导弹M1遮蔽时间为{fun(theta[4], v_1[4], t_1[4][1], t_2[4][1], FY5, M1)}")
print(f"无人机FY1第二枚烟幕弹对导弹M2遮蔽时间为{fun(theta[0], v_1[0], t_1[0][1], t_2[0][1], FY1, M2)}")
print(f"无人机FY2第二枚烟幕弹对导弹M2遮蔽时间为{fun(theta[1], v_1[1], t_1[1][1], t_2[1][1], FY2, M2)}")
print(f"无人机FY3第二枚烟幕弹对导弹M2遮蔽时间为{fun(theta[2], v_1[2], t_1[2][1], t_2[2][1], FY3, M2)}")
print(f"无人机FY4第二枚烟幕弹对导弹M2遮蔽时间为{fun(theta[3], v_1[3], t_1[3][1], t_2[3][1], FY4, M2)}")
print(f"无人机FY5第二枚烟幕弹对导弹M2遮蔽时间为{fun(theta[4], v_1[4], t_1[4][1], t_2[4][1], FY5, M2)}")
print(f"无人机FY1第二枚烟幕弹对导弹M3遮蔽时间为{fun(theta[0], v_1[0], t_1[0][1], t_2[0][1], FY1, M3)}")
print(f"无人机FY2第二枚烟幕弹对导弹M3遮蔽时间为{fun(theta[1], v_1[1], t_1[1][1], t_2[1][1], FY2, M3)}")
print(f"无人机FY3第二枚烟幕弹对导弹M3遮蔽时间为{fun(theta[2], v_1[2], t_1[2][1], t_2[2][1], FY3, M3)}")
print(f"无人机FY4第二枚烟幕弹对导弹M3遮蔽时间为{fun(theta[3], v_1[3], t_1[3][1], t_2[3][1], FY4, M3)}")
print(f"无人机FY5第二枚烟幕弹对导弹M3遮蔽时间为{fun(theta[4], v_1[4], t_1[4][1], t_2[4][1], FY5, M3)}")

print(f"无人机FY1第三枚烟幕弹对导弹M1遮蔽时间为{fun(theta[0], v_1[0], t_1[0][2], t_2[0][2], FY1, M1)}")
print(f"无人机FY2第三枚烟幕弹对导弹M1遮蔽时间为{fun(theta[1], v_1[1], t_1[1][2], t_2[1][2], FY2, M1)}")
print(f"无人机FY3第三枚烟幕弹对导弹M1遮蔽时间为{fun(theta[2], v_1[2], t_1[2][2], t_2[2][2], FY3, M1)}")
print(f"无人机FY4第三枚烟幕弹对导弹M1遮蔽时间为{fun(theta[3], v_1[3], t_1[3][2], t_2[3][2], FY4, M1)}")
print(f"无人机FY5第三枚烟幕弹对导弹M1遮蔽时间为{fun(theta[4], v_1[4], t_1[4][2], t_2[4][2], FY5, M1)}")
print(f"无人机FY1第三枚烟幕弹对导弹M2遮蔽时间为{fun(theta[0], v_1[0], t_1[0][2], t_2[0][2], FY1, M2)}")
print(f"无人机FY2第三枚烟幕弹对导弹M2遮蔽时间为{fun(theta[1], v_1[1], t_1[1][2], t_2[1][2], FY2, M2)}")
print(f"无人机FY3第三枚烟幕弹对导弹M2遮蔽时间为{fun(theta[2], v_1[2], t_1[2][2], t_2[2][2], FY3, M2)}")
print(f"无人机FY4第三枚烟幕弹对导弹M2遮蔽时间为{fun(theta[3], v_1[3], t_1[3][2], t_2[3][2], FY4, M2)}")
print(f"无人机FY5第三枚烟幕弹对导弹M2遮蔽时间为{fun(theta[4], v_1[4], t_1[4][2], t_2[4][2], FY5, M2)}")
print(f"无人机FY1第三枚烟幕弹对导弹M3遮蔽时间为{fun(theta[0], v_1[0], t_1[0][2], t_2[0][2], FY1, M3)}")
print(f"无人机FY2第三枚烟幕弹对导弹M3遮蔽时间为{fun(theta[1], v_1[1], t_1[1][2], t_2[1][2], FY2, M3)}")
print(f"无人机FY3第三枚烟幕弹对导弹M3遮蔽时间为{fun(theta[2], v_1[2], t_1[2][2], t_2[2][2], FY3, M3)}")
print(f"无人机FY4第三枚烟幕弹对导弹M3遮蔽时间为{fun(theta[3], v_1[3], t_1[3][2], t_2[3][2], FY4, M3)}")
print(f"无人机FY5第三枚烟幕弹对导弹M3遮蔽时间为{fun(theta[4], v_1[4], t_1[4][2], t_2[4][2], FY5, M3)}")
